/**
 ******************************************************************************
 * Copyright (c) 2022 - ~, SCUT-RobotLab Development Team
 * @file    Control_Node.h
 * @author   LJY
 * @brief
 ******************************************************************************
 */
#ifndef CONTROL_NODE_H
#define CONTROL_NODE_H

/* Includes ------------------------------------------------------------------*/
#include "ros/ros.h"
#include "robot/RobotRunner.h"
#include "srpq_controller/SRPQ_Controller.h"
#include "common/LegData.h"
#include "common/RobotCommand.h"
#include "common/RCCommand.h"
#include "common/CheaterState.h"
#include "common/TouchForce.h"
#include "common/ImuData.h"
#include "sensor_msgs/JointState.h"
#include "geometry_msgs/Twist.h"

/* Private macros ------------------------------------------------------------*/
/* Private type --------------------------------------------------------------*/
typedef enum {
  Graph_Type1 = 0,
  Graph_Type2,
  Graph_Type3,
  Graph_Type4,
} Debug_GraphType;

typedef enum {
  Graph_Num1 = 0,
  Graph_Num2,
  Graph_Num3,
} Debug_NumType;

typedef struct Route_Cmd
{
  float x;
  float y;
  float pro_yaw;
  float yaw;
}Route_Cmd;


/* Exported function declarations --------------------------------------------*/
class Route_Planning{
public:
  Route_Planning(RobotRunner* robotRunner) : _robotRunner(robotRunner){}
  ~Route_Planning(){}
  void run(void);
  void addTarget(float* data, uint8_t type); // 0 直线  1 曲线
private:
  std::queue<Route_Cmd> routes;
  RobotRunner* _robotRunner;
  Route_Cmd target;
  float MAX_ERR_X = 0.05;
  float MAX_ERR_Y = 0.05;
  float MAX_ERR_YAW = 0.036;
  Mat4<float> T;
  uint8_t process_flag = 2; // 0 位置逼近 1 角度逼近  2 完成
  void calculateTransitionMatrix(Mat3<float> R, Vec3<float> P);
  float limit(float in, float min, float max);
};


class Control_Node {
public:
    Control_Node(ros::NodeHandle* _n, bool _sim) : n(_n), sim(_sim) { Node_Init(); }
    void Run(void);
    void updateRobotCommand(const RobotCommand* cmd);
    void Draw_debugData(Debug_GraphType graph_type, Debug_NumType graph_num, float value, std::string name);

private:
    ros::NodeHandle* n;
    ros::Publisher RobotCmd_Puber;
    ros::Publisher AUcmd_Puber;
    ros::Publisher debugData_Puber;
    ros::Subscriber LegData_Suber;
    ros::Subscriber RCCmd_Suber;
    ros::Subscriber CheaterState_Suber;
    ros::Subscriber ImuData_Suber;
    ros::Subscriber TouchForce_Suber;
    ros::Subscriber Nav_Suber;
    RobotRunner* _robotRunner;
    bool sim = true;  // 控制开启仿真还是实体控制
    bool LegData_Available = false;
    bool CheaterState_Available = false;
    bool RCCmd_Available = false;
    bool ImuData_Available = false;
    bool Estimator_Available = false;
    bool Nav_Enable = false;
    sensor_msgs::JointState DebugData;
    ros::Timer freq_timer;
    ros::Timer debug_timer;
    uint64_t iter = 0;
    uint64_t freq_count = 0;
    uint8_t _au_cmd;
    bool legdataUpdate = false;
    
    void LegData_Callback(const common::LegDataConstPtr& msg);
    void RCCmd_Callback(const common::RCCommandConstPtr& msg);
    void CheaterState_Callback(const common::CheaterStateConstPtr& msg);
    void TouchForce_Callback(const common::TouchForceConstPtr& msg);
    void ImuData_Callback(const common::ImuDataConstPtr& msg);
    void Navigation_Callback(const geometry_msgs::TwistConstPtr& msg);
    void Node_Init(void);
    void FreqTimer_Callback(const ros::TimerEvent &event);
    void DebugTimer_Callback(const ros::TimerEvent &event);
    
};

#endif
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
